function [q]=rot2quater(R)
%[m h]=max([R(1,1)+R(2,2)+R(3,3) R(1,1)-R(2,2)-R(3,3) -R(1,1)+R(2,2)-R(3,3) -R(1,1)-R(2,2)+R(3,3)]);
%if(h==1)
%S=sqrt(R(1,1)+R(2,2)+R(3,3)+1);
%q=[S/4 [R(2,3)-R(3,2) R(3,1)-R(1,3) R(1,2)-R(2,1)]/S];
%end

theta=acos((trace(R)-1)/2);
om=1/[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)];
q=[cos(theta/2) om(:)'/norm(om)*sin(theta/2)];
return;
q=rand(4,1)
q=q/norm(q)
[R]=quater2rot(q)
[q1]=rot2quater(R)'

